#pragma once
#include <iostream>
#include <unordered_map>
#include <vector>
#include <string>
#include <cmath>
#include <yaml-cpp/yaml.h>

extern std::string g_map_file_;
extern std::string g_vehicle_param_path_;

extern float g_movefsm_tofinish_dis_precision;
extern float g_movefsm_tofinish_angle_precision;

extern float g_pose_control_accuracy;// 终点纵向控制精度

extern float g_lat_error_weight_load;
extern float g_heading_error_weight_load;
extern float g_lat_error_rate_weight_load;
extern float g_heading_error_rate_weight_load; 

extern float g_lat_error_weight;
extern float g_heading_error_weight;
extern float g_lat_error_rate_weight;
extern float g_heading_error_rate_weight;

extern float g_lon_preveiwer_dis;
extern std::string g_speed_plan_type;// 直线使用的加减速模型
extern float g_slow_down_distance;
extern float g_slow_down_speed;
extern float g_position_control_distance;
extern float g_position_control_kp;

extern float g_max_acc;//最大加速度
extern float g_max_dcc;//最大减加速度

extern float g_headfollowing_min_radius;
extern float g_turn_min_radius;

extern float g_curvatrue_control_weight;
extern float g_max_rotating_angle_speed;
extern float g_max_rotating_angle_speed_inload;
extern float g_max_control_angle_speed;
extern float g_min_rotating_angle_speed;

extern float g_max_angle_speed_acc;
extern float g_max_angle_speed_dcc;

extern float g_f2r_heading_error;
extern float g_r2f_heading_error;

extern float g_zero_speed;//零速阈值
extern float g_min_move_speed;
extern float g_dec_vel_error_k;
extern float g_dec_response_delay;
extern float g_f2r_pose_thea_path_angle_diff_thresh;
extern float g_search_path_id_dis_thresh;
extern std::vector<std::pair<float, float>> g_v_search_path_id_angle_thresh;
extern float g_search_path_id_angle_thresh;
extern float g_pick_up_length;
extern float g_pathid_valid_dis;
extern float g_online_angle;

extern float g_cfc_rotating_kp;

// cfc f2r s2f f2s judge threshold
extern float g_f2r_pose_thea_path_angle_diff_thresh;
// threshold of next line angle with steer angle in global coord.
extern float g_f2s_line_theta_steer_angle_thresh;
// threshold of steer angle diff with headfollowing speed toward
extern float g_f2s_actuator_steer_speed_toward_threshold;
// threshold of cur angle diff with point targe angle
extern float g_f2s_cur_angle_target_angle_threshold;
// steer to follow
extern float g_s2f_steer_angle_speed_toward_diff_threshold;
extern float g_s2f_global_steer_angle_diff_threshold;
extern float g_s2f_speed_toward_line_angle_diff_threshold;

extern float g_cfc_max_adjust_speed_toward;
extern float g_cfc_max_speed_toward;

extern std::map<float, float> g_cfc_heading_error_weight_map_;
extern float g_creep_distance;

void LoadGlobalParams(const std::string &path, const std::string &yaml_file);